#ifndef __DRV_ALGO_H
#define __DRV_ALGO_H
#include "main.h"

#define PI                          3.14f
#define max(x, y)                   (((x) > (y)) ? (x) : (y))
#define min(x, y)                   (((x) < (y)) ? (x) : (y))

typedef struct {
    // 基础PID参数
    float Kp;
    float Ki;
    float Kd;
    float error_int;        // 积分项
    float last_error;       // 上次误差
    
    // 双向限幅参数
    float max_iout;         // 积分输出限幅（双向，绝对值）
    float max_out;          // 总输出限幅（双向，绝对值）
    
} algo_pid_t;

void Algo_PID_Init(algo_pid_t* pPID, float Kp, float Ki, float Kd, float max_iout, float max_out);
float Algo_PID(algo_pid_t* pPID, float actual, float target);

int float_to_uint(float x_float, float x_min, float x_max, int bits);
float uint_to_float(int x_int, float x_min, float x_max, int bits);
#endif
